package robot;

import lejos.navigation.CompassPilot;
import lejos.navigation.Pilot;
import lejos.nxt.Motor;
import lejos.nxt.addon.CompassSensor;

public class ImprovedCompassPilot extends CompassPilot {

	private final float _turnRatio;
	
	public ImprovedCompassPilot(CompassSensor compass, float wheelDiameter,
			float trackWidth, Motor leftMotor, Motor rightMotor) {
		super(compass, wheelDiameter, trackWidth, leftMotor, rightMotor);
		_turnRatio = trackWidth/wheelDiameter;
	}
	
	public void calibrate() {
		int spd = _speed;
		setSpeed(100);
		regulateSpeed(true);
		compass.startCalibration();
	    int ta = (int)( 720*_turnRatio);
	    _left.rotate(-ta,true);
	    _right.rotate(ta,true);
	    while(isMoving()) Thread.yield();
		compass.stopCalibration();
		setSpeed(spd);
	}

}
